/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "logicmotor.h"
#include <qdebug.h>
#include "ethercatVendorId.h"

LogicMotor::LogicMotor(E_DEVICE_PROTOCOL protocolIn, int aliasIn, int busIdIn)
{
    protocol=protocolIn;
    alias=aliasIn;
    busId=busIdIn;
    controlMode=E_MOTOR_CONTROL_START;

    controlWord = 0x06 ;  // 0x6040 - 控制字
    modeOfOperation=eServoModeCsp ; // 0x6060 - 控制模式
    modeOfOperation_old=modeOfOperation;
    targetPosition=0; // 0x607A - 目标位置 csp
    targetTorque=0; // 0x6071 - 目标力距 cst
    targetVelocity=0; // 0x60FF - 目标速度 csv
    torqueOffset=0; // 0x60B2 - 力距漂移
    velocityOffset=0; // 0x60B1 - 速度漂移

    /*--PDO_INPUT------------------------------------------*/
    errorCode=0; // 0x603F - 错误码 u16
    stateWord=0; // 0x6041 - 状态字 u16
    actPosition=0;  // 0x6064 - 位置反馈 s32
    actVelocity=0;  // 0x606C - 速度反馈 s32
    actTorque=0;  // 0x6077 - 力矩反馈 s16
    actFollowErr=0; // 0x60F4 - 跟随误差 s32
    disModeOfOperation=0; // 0x6061 - 控制模式显示 s8
    digIn=0; // 0x60FD - 数字输入 u32
    digOut=0;// 0x60FE - 数字输out u32

    homeOffset=0;
    motorStateCommand=(E_MOTOR_CMD)0;
    vendorId=D_VENDOR_ID_VIRTUAL;

    pulsePerRound=-1;
    tmpTickCount=0;
}

int LogicMotor::checkEthercatDeviceError(bool &isWarningState,bool &isFaultOccurred,
                                         unsigned int &errorCodeOut,unsigned int &vendorIdOut,int &aliasOut)
{

    errorCodeOut=errorCode;
    vendorIdOut=vendorId;
    aliasOut=alias;
    //pdo
    if( errorCode!=0  && (stateWord&0x08) )
    {//error
        if(!isFaultOccurred )
        {
            if( D_VENDOR_ID_YiLaiSi!=vendorId)
            {
                isFaultOccurred= true;
                return 0;
//                addMsg(ENUM_MSG_ERROR, "RobotBase", "EcatMotor", 5034,
//                       robotInfo.robotId,
//                       i,
//                       ethercatAxis.at(i)->errorCode,
//                       ethercat_node_info.at(i)->devInfo.vendorId );
                // printf( "ddMsg(ENUM_MSG_ERROR, %d, %d",  axis.at(i)->errorCode );
            }
            else
            {
                if(0xff00==errorCode)
                {
                    //临时屏蔽
//                    addMsg(ENUM_MSG_WARNING, "RobotBase", "EcatMotor", 5034,
//                           robotInfo.robotId,
//                           i,
//                           ethercatAxis.at(i)->errorCode,
//                           ethercat_node_info.at(i)->devInfo.vendorId );
                }
                else
                {
                    isFaultOccurred= true;
                    return 0;
//                    addMsg(ENUM_MSG_ERROR, "RobotBase", "EcatMotor", 5034,
//                           robotInfo.robotId,
//                           i,
//                           ethercatAxis.at(i)->errorCode,
//                           ethercat_node_info.at(i)->devInfo.vendorId );
                }
            }


        }
    }
    else if( errorCode!=0 && stateWord&0x80 )
    {//warm

        if(!isWarningState)
        {
            isWarningState = true;
            return 0;
//            addMsg(ENUM_MSG_WARNING, "RobotBase", "EcatMotor", 5041,
//                   robotInfo.robotId,
//                   i,
//                   ethercatAxis.at(i)->errorCode,
//                   ethercat_node_info.at(i)->devInfo.vendorId );
            //  printf( "dxxxxdMsg(ENUM_MSG_ERROR, %d, %d",  axis.at(i)->errorCode );
        }
    }
    else if( D_VENDOR_ID_QingNengDeChuang==vendorId && stateWord&0x80 )
    {//warm　获取具体告警代码的对象字典0x2224

        if(tmpTickCount<5000)
        {
           tmpTickCount++;
        }

        if(!isWarningState    && 5000==tmpTickCount )
        {
            isWarningState = true;
            return 0;
//            addMsg(ENUM_MSG_WARNING, "RobotBase", "EcatMotor", 5609,
//                   robotInfo.robotId,
//                   i,
//                   ethercatAxis.at(i)->errorCode,
//                   ethercat_node_info.at(i)->devInfo.vendorId );
            //  printf( "dxxxxdMsg(ENUM_MSG_ERROR, %d, %d",  axis.at(i)->errorCode );
        }
    }
    else
    {

    }
    return 1;
}

int LogicMotor::checkCanopenDeviceError(bool &isWarningState, bool &isFaultOccurred, unsigned int &errorCodeOut,
                                        unsigned int &vendorIdOut, int &aliasOut)
{
    errorCodeOut=errorCode;
    vendorIdOut=vendorId;
    aliasOut=alias;
    //pdo
    if( errorCode!=0  && (stateWord&0x08) )
    {//error
        if(!isFaultOccurred   )
        {
            isFaultOccurred= true;
            if(E_VENDOR_ID_CURTIS2MOTOR==vendorId
                    && 83==errorCode)
            {
                return 0;
//                addMsg(ENUM_MSG_WARNING, "RobotBase", "EcatMotor", 5034,
//                       robotInfo.robotId,
//                       i,
//                       canopenAxis.at(i)->errorCode,
//                       canopenAxis.at(i)->vendorId );
                // printf( "ddMsg(ENUM_MSG_ERROR, %d, %d",  axis.at(i)->errorCode );
            }
            else
            {
                return 0;
//                addMsg(ENUM_MSG_ERROR, "RobotBase", "EcatMotor", 5034,
//                       robotInfo.robotId,
//                       i,
//                       canopenAxis.at(i)->errorCode,
//                       canopenAxis.at(i)->vendorId );
                // printf( "ddMsg(ENUM_MSG_ERROR, %d, %d",  axis.at(i)->errorCode );
            }

        }
    }
    else if( stateWord&0x80 )
    {//warm
        if(!isWarningState  )
        {
            isWarningState = true;
            return 0;
            //  printf( "dxxxxdMsg(ENUM_MSG_ERROR, %d, %d",  axis.at(i)->errorCode );
        }
    }
    else
    {

    }
    return 1;
}

void LogicMotor::writeActVelocity(int actVelocityIn)
{
    if(D_VENDOR_ID_TECHSERVO==vendorId)
    {
        //泰科一体化关节模块，速度命令单位0.1pulse/s, 反馈速度单位比较复杂。反馈编码器80000pulse/s ,减速比101
        //101*10*80000/131072
        actVelocity=actVelocityIn/616.455;
    }
    else
    {
        actVelocity=actVelocityIn;
    }

}

int LogicMotor::setSdoList(QVector<SdoConfig> &sdoConfigListIn)
{
    sdoConfigList=sdoConfigListIn;
    return 1;
}

int LogicMotor::getSdoList(QVector<SdoConfig> &sdoConfigListOut)
{
    sdoConfigListOut=sdoConfigList;
    return 1;
}

E_MOTOR_CMD LogicMotor::getMotorStateCommand()
{
    return motorStateCommand;

}

E_MOTOR_CONTROL_MODE LogicMotor::getControlMode()
{
    return controlMode;
}

int LogicMotor::setControlMode(E_MOTOR_CONTROL_MODE controlModeIn)
{
    qDebug()<<"6060setControlMode"<<controlModeIn<<"alias"<<alias;
    controlMode=controlModeIn;
    switch(controlMode)
    {
    case E_MOTOR_CONTROL_CSP_MODE:
    {
        modeOfOperation=eServoModeCsp ;
        break;
    }
    case E_MOTOR_CONTROL_CSV_MODE:
    {
        modeOfOperation=eServoModeCsv ;
        break;
    }
    case E_MOTOR_CONTROL_CST_MODE:
    {
        modeOfOperation=eServoModeCst ;
        break;
    }
    default:
    {
        return -1;
    }
    }
    modeOfOperation_old=modeOfOperation;
    return 1;
}

int LogicMotor::getVendorId()
{
    return vendorId;

}

int LogicMotor::setVendorId(int vendorIdIn)
{
    vendorId=vendorIdIn;
//    qDebug()<<"LogicMotor::setVendorId vendorId="<<vendorId;
    return 1;

}

int LogicMotor::setZeroReturnParameter(ZeroReturnParameter zeroReturnParaIn)
{
    zeroReturnPara=zeroReturnParaIn;
    return 1;

}

int LogicMotor::changeControlMode(E_MOTOR_CONTROL_MODE controlModeIn)
{
    controlMode=controlModeIn;
    return 1;
}

int LogicMotor::getAlias()
{
    return alias;
}

int LogicMotor::getProtocol()
{
    return protocol;
}

void LogicMotor::setMotorState(E_MOTOR_CMD cmd)
{
//    qDebug()<<"setMotorState"<<cmd<<isServoOn();
    if(isServoOn() && eServoFaultReset==cmd)
    {
        qDebug()<<"warn: servon on state,can not change to  eServoFaultReset";
        return ;
    }
    motorStateCommand=cmd;
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {
        setModbusMotorState(cmd);
        return ;
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT || protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {
        setEthercatMotorState(cmd);
        return ;
    }

    return ;



}

void LogicMotor::setEthercatMotorState(E_MOTOR_CMD cmd)
{
//    qDebug()<<"setEthercatMotorState"<<cmd;
    switch(cmd)
    {
    case eServoOff: // init value: If in Switch_On_Disabled state, change to Ready_To_Switch On
    {
//        if(controlWord | 0x80)
//            controlWord = 0x06|0x80;
//        else
            controlWord = 0x06;  // operation: Shut down
        break;
    }
    case eServoSwitchOn:  // 伺服准备
    {//motorStateCommand被修改了LogicMotor::updateEthercatMotorState()
        if((stateWord & 0x0F) == 0x00 )
        {
            controlWord = 0x06;  // operation: Shut down
        }
        else if ( (stateWord&0x006f) == 0x0021 )  // FSA State: Ready to Switch On
        {
            controlWord = 0x07;  // operation: Switch On
        }
        break;
    }
    case eServoEnable:  // 上使能
    {//motorStateCommand被修改了LogicMotor::updateEthercatMotorState()
        if(E_VENDOR_ID_ROBOTIQ==vendorId || E_VENDOR_ID_ROBOTIQ2MOTOR==vendorId
                || E_VENDOR_ID_CURTIS2MOTOR==vendorId
                )//兴颂驱动器，在发送pdo时，把控制字转换为兴颂的控制字
        {
            controlWord = 109;
        }
        else
        {
//            if(E_MOTOR_CONTROL_CSP_MODE==controlMode)
//            {
//                controlWord = 0x3f;   // Enable Operation. ３Ｆ 绝对定位方式,5F 相对定位
//            }
//            else
//            {
               controlWord = 0x0f;   // Enable Operation
//            }
        }

        break;
    }
    case eServoDisable:  // 去使能
    {
//        if(controlWord | 0x80)
//            controlWord = 0x07|0x80;
//        else
            controlWord = 0x07; // Disable operation
        break;
    }
    case eServoQuickStop:
    {
//        if(controlWord | 0x80)
//            controlWord = 0x0b|0x80;
//        else
            controlWord = 0x0b; // 急停， Quick Stop
        break;
    }
    case eServoFaultReset:  // 错误恢复, 错误恢复后需要重新伺服准备和上使能
    {
        qDebug()<<"eServoFaultReset";
        controlWord=0;
        usleep(100000);
        controlWord=0x80;
        usleep(100000);
        controlWord=0x06;

        break;
    }
    case eServoHalt:  // 暂停 Halt On; Warning!!! 在暂停驱动器之前确保控制器已经停止发送位置指令
    {
        controlWord = controlWord | 0x0100;
        break;
    }
    case eServoHaltReset:  // 取消暂停 Halt Off, Warning!!!
    {
        controlWord = controlWord & 0xfeff;
        break;
    }
    default:
        break;
    }
    qDebug()<<"setEthercatMotorState  controlWord 0x6040="<< controlWord<<"controlMode"<<controlMode
           <<"alias"<<alias;
}

void LogicMotor::setModbusMotorState(E_MOTOR_CMD cmd)
{
    switch(cmd)
    {
    case eServoOff:
    {
        controlWord=(unsigned short)eServoOff;
        break;
    }
    case eServoSwitchOn:  // 伺服准备
    {
        controlWord=(unsigned short)eServoSwitchOn;
        break;
    }
    case eServoEnable:  // 上使能
    {
        controlWord=(unsigned short)eServoEnable;
        break;
    }
    case eServoDisable:  // 去使能
    {
        controlWord=(unsigned short)eServoDisable;
        break;
    }
    case eServoQuickStop:
    {
        controlWord=(unsigned short)eServoQuickStop;
        break;
    }
    case eServoFaultReset:  // 错误恢复, 错误恢复后需要重新伺服准备和上使能
    {
        controlWord=(unsigned short)eServoFaultReset;
        break;
    }
    case eServoHalt:  // 暂停 Halt On; Warning!!! 在暂停驱动器之前确保控制器已经停止发送位置指令
    {
        controlWord=(unsigned short)eServoHalt;
        break;
    }
    case eServoHaltReset:  // 取消暂停 Halt Off, Warning!!!
    {
        controlWord=(unsigned short)eServoHaltReset;
        break;
    }
    default:
        break;
    }
}

void LogicMotor::updateMotorState()
{

    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {
        updateModbusMotorState();
        return ;
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT || protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {
       updateEthercatMotorState();
       return ;
    }

    return ;


}

void LogicMotor::updateEthercatMotorState()
{
    //not used 不使用了，作废－－－－－－－－－－－－－－－－－－－－－－－－－－－
    return ;
    if(0)
    {
        if( (errorCode!= 0) || (stateWord&0x0008) )
        {
            if( motorStateCommand==eServoFaultReset )
            {
                controlWord |=  0x0080;
            }
            motorStateCommand=(E_MOTOR_CMD)0xFF;
            return;
        }
        else
        {
            if( controlWord&0x80 )
            {
                controlWord &= 0xff7f;
            }
        }
    }


    switch(motorStateCommand)
    {
    case eServoSwitchOn:  // 伺服准备
    {
        if((stateWord & 0x0F) == 0x00 )
        {
            controlWord = 0x06;  // operation: Shut down
        }
        else if ( (stateWord&0x006f) == 0x0021 )  // FSA State: Ready to Switch On
        {
            controlWord = 0x07;  // operation: Switch On
            motorStateCommand=(E_MOTOR_CMD)0xFF;
        }
        break;
    }
    case eServoEnable:  // 上使能
    {
//        if( (stateWord&0x006f) == 0x0023 )  // FSA State: Switch On || D_VENDOR_ID_TECHSERVO==vendorId
//        {
//            targetPosition = encoderValue;
            if(E_MOTOR_CONTROL_CSP_MODE==controlMode)
            {
                controlWord = 0x3f;   // Enable Operation. ３Ｆ 绝对定位方式,5F 相对定位
            }
            else
            {
               controlWord = 0x0f;   // Enable Operation
            }

            qDebug()<<"eServoEnable "<<controlWord;

//            motorStateCommand=(E_MOTOR_CMD)0xFF;
//        }
//        else if((stateWord & 0x0F) == 0x00 )
//        {
//            controlWord =  0x06;  // operation: Shut down
//        }
//        else if ( (stateWord&0x006f) == 0x0021 )  // FSA State: Ready to Switch On
//        {
//            controlWord = 0x07;  // operation: Switch On
//        }
        break;
    }
    default:
        break;
    }
}

void LogicMotor::updateModbusMotorState()
{

    return ;
}

bool LogicMotor::isServoOn(  )
{
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {

        return isServoOn_modbus(  );
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT)
    {

       return isServoOn_ethercat();
    }
    else if(protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {

       return isServoOn_ethercat();
    }

    return false;
}

bool LogicMotor::isServoQuickStop(   )
{
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {

        return isServoQuickStop_modbus(  );
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT || protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {

       return isServoQuickStop_ethercat();
    }
    return false;
}

bool LogicMotor::isServoPowerOn(  )
{
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {

        return isServoPowerOn_modbus(  );
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT|| protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {

       return isServoPowerOn_ethercat();
    }
    return false;
}

bool LogicMotor::isServoFllowingError(  )
{
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {

        return isServoFllowingError_modbus(  );
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT|| protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {

       return isServoFllowingError_ethercat();
    }
    return false;
}

bool LogicMotor::isServoTargetReached(  )
{
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {

        return isServoTargetReached_modbus(  );
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT|| protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {

       return isServoTargetReached_ethercat();
    }
    return false;
}

bool LogicMotor::isServoHoming(  )
{
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {

        return isServoHoming_modbus(  );
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT|| protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {

       return isServoHoming_ethercat();
    }
    return false;
}

bool LogicMotor::isServoHomingOk(  )
{
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {

        return isServoHomingOk_modbus(  );
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT|| protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {

       return isServoHomingOk_ethercat();
    }
    return false;
}

bool LogicMotor::isServoHomingErr(  )
{
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {

        return isServoHomingErr_modbus(  );
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT|| protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {

       return isServoHomingErr_ethercat();
    }
    return false;
}




bool LogicMotor::isServoOn_ethercat(  )
{
    return (stateWord&0x67)==0x27?true:false;
}

bool LogicMotor::isServoQuickStop_ethercat(   )
{
    return (stateWord&0x67)==0x07?true:false;
}

bool LogicMotor::isServoPowerOn_ethercat(  )
{
    return (stateWord&0x10)==0x10?true:false;
}

bool LogicMotor::isServoFllowingError_ethercat(  )
{
    return (stateWord&0x2000)==0x2000?true:false;
}

bool LogicMotor::isServoTargetReached_ethercat(  )
{
    return (stateWord&0x400)==0x400?true:false;
}

bool LogicMotor::isServoHoming_ethercat(  )
{
    return false;
}

bool LogicMotor::isServoHomingOk_ethercat(  )
{
//    return (stateWord&0x8000)==0x8000?true:false;
    return (stateWord&0x1000)==0x1000?true:false;
}

bool LogicMotor::isServoHomingErr_ethercat(  )
{

    return (stateWord&0x2000)==0x2000?true:false;
}



bool LogicMotor::isServoOn_modbus(  )
{
//    return true;
    return stateWord==(unsigned short)eServoEnable?true:false;
}

bool LogicMotor::isServoQuickStop_modbus(   )
{
    return stateWord==eServoQuickStop?true:false;
}

bool LogicMotor::isServoPowerOn_modbus(  )
{
    return false;
}

bool LogicMotor::isServoFllowingError_modbus(  )
{
    return false;
}

bool LogicMotor::isServoTargetReached_modbus(  )
{
    return false;
}

//int LogicMotor::changeToCsvMode()
//{
//    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
//    {

//        return changeToCsvMode_modbus(  );
//    }
//    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT|| protocol==E_DEVICE_PROTOCOL_CANOPEN)
//    {

//       return changeToCsvMode_ethercat();
//    }

//    return false;
//}

//int LogicMotor::changeToCsvMode_ethercat()
//{
//    modeOfOperation_old=modeOfOperation;
//    modeOfOperation=eServoModeCsv;
//    return 1;
//}

//int LogicMotor::changeToCsvMode_modbus()
//{
//     return -1;
//}

int LogicMotor::changeToHomingMode()
{
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {

        return changeToHomingMode_modbus(  );
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT|| protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {

       return changeToHomingMode_ethercat();
    }

    return false;
}

int LogicMotor::changeToHomingMode_ethercat()
{
    modeOfOperation_old=modeOfOperation;
    modeOfOperation=eServoModeHm;
    controlMode_old=controlMode;
    controlMode=E_MOTOR_CONTROL_HOMING;
    qDebug()<<"changeToHomingMode_ethercat modeOfOperation"<<modeOfOperation;
    return 1;
}

int LogicMotor::changeToHomingMode_modbus()
{
    controlMode_old=controlMode;
    controlMode=E_MOTOR_CONTROL_HOMING;
    return -1;
}


bool LogicMotor::startGoHome(  )
{
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {

        return startGoHome_modbus(  );
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT|| protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {

       return startGoHome_ethercat();
    }

    return false;
}

bool LogicMotor::startGoHome_ethercat()
{

//    controlWord=controlWord&0xfeff;//halt cancel
    controlWord=controlWord|0x0010;//启动回零　第５位控制启动回零。第９位控制暂停回零。
//      controlWord=controlWord&0x0f;//2021啥时候改成这样了？
//    controlWord=0x001f;

//    qDebug()<<"LogicMotor::startGoHome_ethercat"<<controlWord;
    //bit4需要有一个从０到１的跳变，才能启动回零。
//    if(controlWord>>4&1)
//    {
//        controlWord=controlWord&0xffef;
//    }
//    else
//    {

//    }

    return true;
}

bool LogicMotor::startGoHome_modbus()
{
    return false;
}


bool LogicMotor::haltGoHome(  )
{
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {

        return haltGoHome_modbus(  );
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT|| protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {

       return haltGoHome_ethercat();
    }

    return false;
}

bool LogicMotor::haltGoHome_ethercat()
{
    controlWord=controlWord|0x0100;//halt回零
    return true;
}

bool LogicMotor::haltGoHome_modbus()
{
    return false;
}

int LogicMotor::getZeroMovingStatus(EM_ZERO_MOVING_STATUS &statusOut)
{
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {

        return getZeroMovingStatus_modbus( statusOut );
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT|| protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {

       return getZeroMovingStatus_ethercat(statusOut);
    }
    return false;
}

int LogicMotor::getZeroMovingStatus_ethercat(EM_ZERO_MOVING_STATUS &statusOut)
{

    if(true==isServoHomingOk())
    {
        statusOut=EM_ZERO_MOVING_STATUS_FINISHED;
    }
    else if(true==isServoHomingErr())
    {
        statusOut=EM_ZERO_MOVING_STATUS_ERROR;
    }
    else
    {
        statusOut=EM_ZERO_MOVING_STATUS_MOVING;
    }

    return 1;



}

int LogicMotor::getZeroMovingStatus_modbus(EM_ZERO_MOVING_STATUS &statusOut)
{
    return -1;
}

int LogicMotor::restorNormalOperationMode()
{
    if(protocol==E_DEVICE_PROTOCOL_MODBUS_RTU || protocol==E_DEVICE_PROTOCOL_MODBUS_TCP)
    {

        return restorNormalOperationMode_modbus(  );
    }
    else if(protocol==E_DEVICE_PROTOCOL_ETHERCAT|| protocol==E_DEVICE_PROTOCOL_CANOPEN)
    {

       return restorNormalOperationMode_ethercat();
    }
    return false;
}

int LogicMotor::restorNormalOperationMode_ethercat()
{
    controlWord=controlWord&0xffef;//取消回零
    controlWord=controlWord&0xfeff;//halt cancel
    modeOfOperation=modeOfOperation_old;

    controlMode=controlMode_old;

    qDebug()<<"restorNormalOperationMode_ethercat modeOfOperation"<<modeOfOperation;
    return 1;

}

int LogicMotor::restorNormalOperationMode_modbus()
{
    controlMode=controlMode_old;
    return -1;
}


bool LogicMotor::isServoHoming_modbus(  )
{
    return false;
}

bool LogicMotor::isServoHomingOk_modbus(  )
{
    return false;
}

bool LogicMotor::isServoHomingErr_modbus(  )
{
    return false;
}

void LogicMotor::setControlWordDebug(unsigned short controlWordIn)
{
    controlWord=controlWordIn;
    qDebug()<<"setControlWordDebug"<<controlWord<<"alias"<<alias;
}
